/*
 * ConcreteHardwareController.cpp
 *
 *  Created on: 13 dec. 2012
 *      Author: Admin
 */

#include "ConcreteHardwareController.hpp"

#include "Engine.hpp"
#include "Robot.hpp"
#include "State.hpp"

#include <util/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#define LED1 (1 << PD1)
#define LEFTWHEEL (1 << PD2)
#define CONTROL (1 << PD3)
#define RIGHTWHEEL (1 << PD4)

#define DISTANCE (1<<PB0)
#define EMERGENCY (1<<PB1)
#define COLLISIONFRONT (1<<PB2)
#define COLLISIONBACK (1<<PB3)

#define COLORLANELEFT (1<<PB4)
#define COLORLANERIGHT (1<<PB5)
#define CROSSROADS (1<<PB6)

ConcreteHardwareController::ConcreteHardwareController()
{
	//Initialize Hardware:

	// PINB = input
	DDRB = 0x00;
	// PINC = input
	DDRC = 0x00;
	// PORTD = output
	DDRD = 0xFF;

	// Pin change interrupt enable 0, PCIE = 0
	PCICR = (1<<PCIE0);

	// Pin PCINT0 (pin 0) responsive, PCINT0 = 0
	PCMSK0 = (1<<PCINT1);

	// Enable interrupts
	sei();
}

void delay_ms(unsigned int time)
{
	while (time--)
		_delay_ms(1);
}

void delay_ms(unsigned int* time)
{
	while ((*time)--)
		_delay_ms(1);
}

HardwareController& ConcreteHardwareController::getHardwareController()
{
	static ConcreteHardwareController ioManager;
	return ioManager;
}

void ConcreteHardwareController::wait(unsigned int ms)
{
	delay_ms(ms);
}

void ConcreteHardwareController::wait(unsigned int* ms)
{
	delay_ms(ms);
}

void ConcreteHardwareController::activateWheels()
{
	PORTD |= CONTROL;
}

void ConcreteHardwareController::disableWheels()
{
	PORTD &= ~CONTROL;
}

void ConcreteHardwareController::leftWheelForward()
{
	PORTD |= LEFTWHEEL;
}

void ConcreteHardwareController::leftWheelBackward()
{
	PORTD &= ~LEFTWHEEL;
}

void ConcreteHardwareController::rightWheelForward()
{
	PORTD |= RIGHTWHEEL;
}

void ConcreteHardwareController::rightWheelBackward()
{
	PORTD &= ~RIGHTWHEEL;
}

ISR(PCINT0_vect/*, ISR_NAKED*/)
{
//	//Disable interrupts
	cli();
	//Store current engine state
	Engine::State engineState = Robot::getInstance().getEngine().getState();
	//Stop engine
	Robot::getInstance().getEngine().stop();
	//Wait for emergency stop disabled
	while (ConcreteHardwareController::getHardwareController().pollEmergencyStop())
	{
		//While emergency stop activated, blink LED
		ConcreteHardwareController::getHardwareController().wait(50);
		ConcreteHardwareController::getHardwareController().activateLED();
		ConcreteHardwareController::getHardwareController().wait(50);
		ConcreteHardwareController::getHardwareController().disableLED();
	}
	//Restore engine state
	Robot::getInstance().getEngine().setState(engineState);
	sei();

//	//Activate emergency stop
//	EmergencyStopActivatedEvent event;
//	Robot::getInstance().handleEvent(event);
}

ConcreteHardwareController::~ConcreteHardwareController()
{

}

void ConcreteHardwareController::activateLED()
{
	PORTD |= LED1;
}

void ConcreteHardwareController::disableLED()
{
	PORTD &= ~LED1;
}

bool ConcreteHardwareController::pollCollisionSensorBack()
{
	return ((PINB & COLLISIONBACK) == COLLISIONBACK);
}

bool ConcreteHardwareController::pollCollisionSensorFront()
{
	return !((PINB & COLLISIONFRONT) == 0);
}

bool ConcreteHardwareController::pollCrossRoadsSensor()
{
	return !((PINB & CROSSROADS) == 0);
}

bool ConcreteHardwareController::pollDistanceSensor()
{
	return ((PINB & DISTANCE) == 0);
}

bool ConcreteHardwareController::pollEmergencyStop()
{
	return ((PINB & EMERGENCY) == 0);
}

bool ConcreteHardwareController::pollColorlaneLeft()
{
	return !((PINB & COLORLANELEFT) == 0);
}

bool ConcreteHardwareController::pollColorlaneRight()
{
	return !((PINB & COLORLANERIGHT) == 0);
}
